#include "douglas_peucker.hh"
#include <cmath>

template <typename T>
static inline
T*
next_neigbh(int ncols,
	    T* p,
	    e_dir *dir)
{
  switch (*dir)
    {
    case DIR_RIGHT:
      return p + 1;
    case DIR_RIGHT_UP:
      return p + 1 - ncols;
    case DIR_UP:
      return p - ncols;
    case DIR_LEFT_UP:
      return p - 1 - ncols;
    case DIR_LEFT:
      return p - 1;
    case DIR_LEFT_DOWN:
      return p - 1 + ncols;
    case DIR_DOWN:
      return p + ncols;
    case DIR_RIGHT_DOWN:
      return p + 1 + ncols;
    default:
      return p;
    }
}

static inline
void
dec_dir(e_dir* d)
{
  *d = (e_dir) (((unsigned)*d + 7) % 8);
}

static inline
void
inc_dir(e_dir* d)
{
  *d = (e_dir) (((unsigned)*d + 9) % 8);
}

static inline
void
inverse_dir(e_dir* d)
{
  *d = (e_dir) (((unsigned)*d + 4) % 8);
}

template <typename I>
static inline
typename I::value*
contour_next(const I& ima,
	     typename I::value* origin,
	     e_dir* dir)
{
  int ncols = ima.ncols() + 2 * ima.border();
  typename I::value* neigbh;

  inverse_dir(dir);

  do
    {
      dec_dir(dir);
      neigbh = next_neigbh(ncols, origin, dir);
    }
  while (*neigbh == mln::literal::zero);

  return neigbh;
}

template <typename T>
static inline
unsigned
hook_to_index(const T* hook,
	      const T* first)
{
  return ((unsigned) (hook - first)) / sizeof (T);
}

template <typename I>
static
t_point_list*
code_base(const I& ima,
	  typename I::value* origin,
	  e_dir dir_origin)
{
  e_dir dir = dir_origin;
  e_dir old_dir = dir;
  typename I::value* current = origin;
  typename I::value* old = origin;
  typename I::value* node = origin;
  const typename I::value* first = ima.buffer();

  t_point_list* l = new t_point_list;

  do
    {
      //if (dir != old_dir)
      //{
	  unsigned row = ima.point_at_index(hook_to_index(node, first)).row ();
	  unsigned col = ima.point_at_index(hook_to_index(node, first)).col ();
	  l->push_back (t_point (row, col));
	  node = old;
	  //}

      old = current;
      old_dir = dir;
      current = contour_next(ima, current, &dir);
    }
  while (current != origin);

  unsigned row = ima.point_at_index(hook_to_index(node, first)).row ();
  unsigned col = ima.point_at_index(hook_to_index(node, first)).col ();

  l->push_front (t_point (row, col));

  return l;
}

template <typename I>
t_point_list*
code_base_initialize(I& ima)
{
  unsigned i = 0;
  typename I::value* start = ima.buffer();
  typename I::value* first = start;
  unsigned ncols = ima.ncols() + 2 * ima.border();

  start += ima.border() * (ncols + 1);

  i = 0;

  start += ima.ncols() / 2;

  while ((i < ima.nrows()) && (*start == mln::literal::zero))
    {
      start += ncols;
      ++i;
    }

  return code_base(ima, start, DIR_DOWN);
}


void//I*
draw_polygon(image2d<value::rgb8>& ima,
	     t_point_ord_vector& vect)
{
  if (vect.size() == 0)
    return ;

  t_point prec = vect.back();

  for (t_point_ord_vector::const_iterator i = vect.begin();
       i != vect.end(); ++i)
    {
      draw::line(ima,
		 point2d(i->first, i->second),
		 point2d(prec.first, prec.second),
		 literal::red);
      prec = *i;
    }
}


static inline
double
get_dist_from_line (std::pair<unsigned, unsigned> point_a,
		    int a,
		    int b,
		    int c)
{
  int res = a * point_a.second + point_a.first * b + c;
  //  std::cout << "res = " << res << " et sq = " << sq << std::endl;
  //  res = res / sq;
  //  std::cout << "Apres division, res = " << res << std::endl;



  if (res < 0)
    return - res;
  else
    return res;

}

static inline
t_point
get_max_point (t_point_list	*list,
	       int		a,
	       int		b,
	       int		c,
	       int		&index_max,
	       double		&max)
{
  t_point_list::iterator it;
  double cur_dist;
  max = 0;
  int index;
  t_point res;
  t_point_list::iterator it_max = list->begin ();

  for (it = ++(list->begin ()), index = 0; it != --(list->end ()); ++it, ++index)
    {
      cur_dist = get_dist_from_line (*it, a, b, c);
      if (cur_dist > max)
	{
	  max = cur_dist;
	  it_max = it;
	  index_max = index;
	}
    }

  res = *it_max;
  return res;
}

t_point_ord_vector
dougl_rec (std::list<std::pair<unsigned, unsigned> > list_init,
	   std::pair<unsigned, unsigned> p1,
	   std::pair<unsigned, unsigned> p2,
	   double prec)
{
  t_point_ord_vector res;
  std::pair<unsigned, unsigned> max_point;
  std::list<std::pair<unsigned, unsigned> >::iterator it;
  std::list<std::pair<unsigned, unsigned> > list_inf, list_sup;
  int index_max = 0;
  int i;
  double max = 0;
  t_point_ord_vector res_aux;
  int a = (p2.first - p1.first);
  int b = p1.second - p2.second;
  int c = p1.first * p2.second - p1.second * p2.first;


  max_point = get_max_point (&list_init, a, b, c, index_max, max);
  max = max / (sqrt ((a * a) + (b * b)));

  if (max > prec && list_init.size () > 2)
    {
      for (it = ++(list_init.begin ()), i = 0; it != --(list_init.end ()); ++it, ++i)
	{
	  if (i <= index_max)
	    list_inf.push_back (*it);
	  if (i >= index_max)
	    list_sup.push_back (*it);
	}

      res = dougl_rec (list_inf, list_inf.front (), list_inf.back (), prec);
      res_aux = dougl_rec (list_sup, list_sup.front (), list_sup.back (), prec);
      res.insert (--(res.end ()), res_aux.begin (), res_aux.end ());
      res.pop_back ();
    }
  else
    {
      res.push_back (p1);
      res.push_back (p2);
    }
  return res;
}

t_point_ord_vector
douglas_peucker (t_point_list& list, double prec)
{
  t_point_ord_vector res, res_aux;
  std::pair<unsigned, unsigned> point_init; //first = i, second =j
  t_point_list::iterator it = list.begin ();
  unsigned dist = 0;
  std::list<std::pair<unsigned, unsigned> > list_inf, list_sup;

  for (; it != list.end (); ++it)
    {
      if (dist++ >= list.size () / 2)
	list_sup.push_back (*it);
      else
	list_inf.push_back (*it);
    }

  res = dougl_rec (list_inf, list_sup.front (), list_inf.front (), prec);
  res_aux = dougl_rec (list_sup, list_inf.front (), list_sup.front (), prec);
  res.insert (res.end(), res_aux.begin (), res_aux.end ());
  return (res);
}


int main (int argc, char** argv)
{
  image2d<bool> ima;
  t_point_list* list_of_every_points = new t_point_list;
  t_point_list::iterator it;
  t_point_ord_vector vect;

  if (argc != 3)
    {
      std::cout << "Usage : ./polygons image nb_of_points" << std::endl;
      return 1;
    }

  io::pbm::load (ima, argv[1]);

  list_of_every_points = code_base_initialize (ima);


  vect = douglas_peucker (*list_of_every_points, atof(argv[2]));

  value::rgb8 v;
  image2d<value::rgb8> res = data::convert (v, ima);

  draw_polygon (res, vect);

  io::ppm::save(res, "out_" + std::string(argv[2]) + std::string("_") + std::string(argv[1]));

  return 0;
}
